﻿#region Using statements
using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using System.Runtime.InteropServices;
using System.Windows.Forms;
#endregion

namespace SmartFrequency.SoundAnalysis
{
    /// <summary>
    /// Cooley-Tukey FFT algorithm.
    /// </summary>
    class FftAlgorithm
    {
        #region Private variables
        const int BIT_LENGTH = 16;
        const int ODD_LENGTH = 32768;    // 2 ^ BIT_LENGTH;
        // For 24bit 2s operation
        private int[] index_m = new int[] { 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768, 65536, 131072, 262144, 524288, 1048576, 2097152, 4194304, 8388608 };
        // For 16bit 2s operation
        //private int[] index_m = new int[] { 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768};
        //private int[] index_m = new int[BIT_LENGTH];

        // Based on pi = 3.141592, for 8bit 2s operation
        //private double[] index_alpha = new double[] { 3.141592, 1.570796, 0.785398, 0.392699, 0.1963945, 0.09817475, 0.049087375, 0.0245436875 };
        // Based on pi = 3.141592, for 16bit 2s operation
        private double[] index_alpha = new double[] { -3.141592, -1.570796, -0.785398, -0.392699, -0.196349, -0.098174, -0.049087, -0.024543, -0.012271, -0.006135, -0.0030067, -0.001533, -0.000766, -0.000383, -0.000191, -0.000095 };
        //private double[] index_alpha = new double[BIT_LENGTH];

        // Based on e^(-2*pi/N*k), for 16bit 2s operation
        //ComplexNumber[,] index_odd = new ComplexNumber[BIT_LENGTH, ODD_LENGTH];
        double[,] index_odd_re = new double[BIT_LENGTH, ODD_LENGTH];
        double[,] index_odd_im = new double[BIT_LENGTH, ODD_LENGTH];
        #endregion
        
        /*
        #region External functions
        //[DllImport("libfftw3-3.dll")]
        //protected static extern int fftw_plan fftw_plan_dft_r2c_1d(int n, double *in, fftw_complex *out, unsigned flags);
        #endregion
        */

        #region Private functions
        /// <summary>
        /// Gets number of significat bytes.
        /// </summary>
        /// <param name="n">Number</param>
        /// <returns>Amount of minimal bits to store the number.</returns>
        private static int Log2(int n)
        {
            int i = 0;
            while (n > 0)
            {
                ++i; n >>= 1;
            }
            return i;
        }

        /// <summary>
        /// Reverses bits in the number.
        /// </summary>
        /// <param name="n">Number</param>
        /// <param name="bitsCount">Significant bits in the number.</param>
        /// <returns>Reversed binary number.</returns>
        private static int ReverseBits(int n, int bitsCount)
        {
            int reversed = 0;
            for (int i = 0; i < bitsCount; i++)
            {
                int nextBit = n & 1;
                n >>= 1;

                reversed <<= 1;
                reversed |= nextBit;
            }
            return reversed;
        }

        /// <summary>
        /// Checks if number is power of 2.
        /// </summary>
        /// <param name="n">number</param>
        /// <returns>true if n=2^k and k is positive integer</returns>
        private static bool IsPowerOfTwo(int n)
        {
            return n > 1 && (n & (n - 1)) == 0;
        }
        #endregion

        #region Public functions
        public FftAlgorithm()
        {
            int i;

            for (i = 0; i < BIT_LENGTH; i++)
            {
                //index_m[i] = 1 << i;
                //index_alpha[i] = -(2 * Math.PI / (index_m[i] * 2));
                for (int k = 0; k < index_m[i]; k++)
                {
                    //index_odd[i,k] = new ComplexNumber(0, index_alpha[i] * k).PoweredE();
                    index_odd_re[i, k] = Math.Cos(index_alpha[i] * k);      //Math.Exp(Re) * Math.Cos(Im);     ->      (0, index_alpha[i] * k)
                    index_odd_im[i, k] = Math.Sin(index_alpha[i] * k);      //Math.Exp(Re) * Math.Sin(Im);     ->      (0, index_alpha[i] * k)
                }
            }

            /*
            StringBuilder str = new StringBuilder();
            str.AppendFormat("{0} - {1} - {2} - {3} - {4} - {5} - {6} - {7}", index_alpha[0], index_alpha[1], index_alpha[2], index_alpha[3], index_alpha[4], index_alpha[5], index_alpha[6], index_alpha[7]);
            MessageBox.Show(str.ToString());
            str.Remove(0, str.Length);
            str.AppendFormat("{0} - {1} - {2} - {3} - {4} - {5} - {6} - {7}", index_alpha[8], index_alpha[9], index_alpha[10], index_alpha[11], index_alpha[12], index_alpha[13], index_alpha[14], index_alpha[15]);
            MessageBox.Show(str.ToString());
            str.Remove(0, str.Length);
            //str.AppendFormat("{0} - {1} - {2} - {3} - {4} - {5} - {6} - {7}", index_m[16], index_m[17], index_m[18], index_m[19], index_m[20], index_m[21], index_m[22], index_m[23]);
            //MessageBox.Show(str.ToString());
            //str.Remove(0, str.Length);
            */

        }
        /// <summary>
        /// Calculates FFT using Cooley-Tukey FFT algorithm.
        /// </summary>
        /// <param name="x">input data</param>
        /// <returns>spectrogram of the data</returns>
        /// <remarks>
        /// If amount of data items not equal a power of 2, then algorithm
        /// automatically pad with 0s to the lowest amount of power of 2.
        /// </remarks>
        public static double[] Calculate(double[] x)
        {
            int length;
            int bitsInLength;
            if (IsPowerOfTwo(x.Length))
            {
                length = x.Length;
                bitsInLength = Log2(length) - 1;
            }
            else
            {
                bitsInLength = Log2(x.Length);
                length = 1 << bitsInLength;
                // the items will be pad with zeros
            }

            // bit reversal
            ComplexNumber[] data = new ComplexNumber[length];
            for (int i = 0; i < x.Length; i++)
            {
                int j = ReverseBits(i, bitsInLength);
                data[j] = new ComplexNumber(x[i]);
            }
            // BT : Bug fix
            for (int i = 0; i < length; i++)
            {
                if(data[i] == null)
                    data[i]= new ComplexNumber(0);
            }
            // ---------------------
            // Cooley-Tukey 
            for (int i = 0; i < bitsInLength; i++)
            {
                int m = 1 << i;
                int n = m * 2;
                double alpha = -(2 * Math.PI / n);

                for (int k = 0; k < m; k++)
                {
                    // e^(-2*pi/N*k)
                    ComplexNumber oddPartMultiplier = new ComplexNumber(0, alpha * k).PoweredE();

                    for (int j = k; j < length; j += n)
                    {
                        ComplexNumber evenPart = data[j];
                        ComplexNumber oddPart = oddPartMultiplier * data[j + m];
                        data[j] = evenPart + oddPart;
                        data[j + m] = evenPart - oddPart;
                    }
                }
            }

            // calculate spectrogram
            double[] spectrogram = new double[length];
            for (int i = 0; i < spectrogram.Length; i++)
            {
                spectrogram[i] = data[i].AbsPower2();
            }
            return spectrogram;
        }

        /// <summary>
        /// Calculates FFT using Cooley-Tukey FFT algorithm with using Danielson-Lanczos
        /// </summary>
        /// <param name="x">input data</param>
        /// <returns>spectrogram of the data</returns>
        /// <remarks>
        /// If amount of data items not equal a power of 2, then algorithm
        /// automatically pad with 0s to the lowest amount of power of 2.
        /// </remarks>
        public double[] Calculate2(double[] x)
        {
            int length;
            int bitsInLength;
            if (IsPowerOfTwo(x.Length))
            {
                length = x.Length;
                bitsInLength = Log2(length) - 1;
            }
            else
            {
                bitsInLength = Log2(x.Length);
                length = 1 << bitsInLength;
            }

            // bit reversal
            double[] data_re = new double[length];
            double[] data_im = new double[length];
            for (int i = 0; i < x.Length; i++)
            {
                int j = ReverseBits(i, bitsInLength);
                data_re[j] = x[i];
                //data_im[j] is naturally zero by definetion
            }

            // ---------------------
            // Cooley-Tukey 
            double even_re, even_im, odd_re, odd_im;
            for (int i = 0; i < bitsInLength; i++)
            {
                int m = index_m[i];
                int n = index_m[i+1];
                double alpha = index_alpha[i];

                for (int k = 0; k < m; k++)
                {
                    for (int j = k; j < length; j += n)
                    {
                        // Version 1
                        /*
                        even_re = data_re[j];
                        even_im = data_im[j];
                        odd_re = (index_odd_re[i, k] * data_re[j + m]) - (index_odd_im[i, k] * data_im[j + m]);
                        odd_im = (index_odd_im[i, k] * data_re[j + m]) + (index_odd_re[i, k] * data_im[j + m]);
                        data_re[j] = even_re + odd_re;
                        data_im[j] = even_im + odd_im;
                        data_re[j+m] = even_re - odd_re;
                        data_im[j+m] = even_im - odd_im;
                        */
                        // Version 2
                        odd_re = (index_odd_re[i, k] * data_re[j + m]) - (index_odd_im[i, k] * data_im[j + m]);
                        odd_im = (index_odd_im[i, k] * data_re[j + m]) + (index_odd_re[i, k] * data_im[j + m]);
                        data_re[j + m] = data_re[j] - odd_re;
                        data_im[j + m] = data_im[j] - odd_im;
                        data_re[j] += odd_re;
                        data_im[j] += odd_im;
                    }
                }
            }

            // calculate spectrogram
            double[] spectrogram = new double[length];
            for (int i = 0; i < spectrogram.Length; i++)
            {
                spectrogram[i] = (data_re[i] * data_re[i]) + (data_im[i] * data_im[i]);
            }
            return spectrogram;
        }


        /// <summary>
        /// Calculates FFT using split radix real FFT algorithm
        /// </summary>
        /// <param name="x">input data</param>
        /// <returns>spectrogram of the data</returns>
        /// <remarks>
        /// If amount of data items not equal a power of 2, then algorithm
        /// automatically pad with 0s to the lowest amount of power of 2.
        /// </remarks>
        public double[] Calculate3(double[] X)
        {
            int length;
            int bitsInLength;
            int I, I0, I1, I2, I3, I4, I5, I6, I7, I8, IS, ID;
            int J, K, M, N, N2, N4, N8;
            double A, A3, CC1, SS1, CC3, SS3, E, R1, XT;
            double T1, T2, T3, T4, T5, T6;

            if (IsPowerOfTwo(X.Length))
            {
                length = X.Length;
                bitsInLength = Log2(length) - 1;
            }
            else
            {
                bitsInLength = Log2(X.Length);
                length = 1 << bitsInLength;
            }
            N = length;

            M = (int)(Math.Log(length) / Math.Log(2.0));               /* N=2^M */

            /* ----Digit reverse counter---------------------------------------------- */
            J = 1;
            for (I = 1; I < N; I++)
            {
                if (I < J)
                {
                    XT = X[J];
                    X[J] = X[I];
                    X[I] = XT;
                }
                K = N / 2;
                while (K < J)
                {
                    J -= K;
                    K /= 2;
                }
                J += K;
            }

            /* ----Length two butterflies--------------------------------------------- */
            IS = 1;
            ID = 4;
            do
            {
                for (I0 = IS; I0 <= N; I0 += ID)
                {
                    I1 = I0 + 1;
                    R1 = X[I0];
                    X[I0] = R1 + X[I1];
                    X[I1] = R1 - X[I1];
                }
                IS = 2 * ID - 1;
                ID = 4 * ID;
            } while (IS < N);

            /* ----L shaped butterflies----------------------------------------------- */
            N2 = 2;
            for (K = 2; K <= M; K++)
            {
                N2 = N2 * 2;
                N4 = N2 / 4;
                N8 = N2 / 8;
                E = (float)6.2831853071719586f / N2;
                IS = 0;
                ID = N2 * 2;
                do
                {
                    for (I = IS; I < N; I += ID)
                    {
                        I1 = I + 1;
                        I2 = I1 + N4;
                        I3 = I2 + N4;
                        I4 = I3 + N4;
                        T1 = X[I4] + X[I3];
                        X[I4] = X[I4] - X[I3];
                        X[I3] = X[I1] - T1;
                        X[I1] = X[I1] + T1;
                        if (N4 != 1)
                        {
                            I1 += N8;
                            I2 += N8;
                            I3 += N8;
                            I4 += N8;
                            T1 = (X[I3] + X[I4]) * .7071067811865475244f;
                            T2 = (X[I3] - X[I4]) * .7071067811865475244f;
                            X[I4] = X[I2] - T1;
                            X[I3] = -X[I2] - T1;
                            X[I2] = X[I1] - T2;
                            X[I1] = X[I1] + T2;
                        }
                    }
                    IS = 2 * ID - N2;
                    ID = 4 * ID;
                } while (IS < N);
                A = E;
                for (J = 2; J <= N8; J++)
                {
                    A3 = 3.0 * A;
                    CC1 = Math.Cos(A);
                    SS1 = Math.Sin(A);  /*typo A3--really A?*/
                    CC3 = Math.Cos(A3); /*typo 3--really A3?*/
                    SS3 = Math.Sin(A3);
                    A = (float)J * E;
                    IS = 0;
                    ID = 2 * N2;
                    do
                    {
                        for (I = IS; I < N; I += ID)
                        {
                            I1 = I + J;
                            I2 = I1 + N4;
                            I3 = I2 + N4;
                            I4 = I3 + N4;
                            I5 = I + N4 - J + 2;
                            I6 = I5 + N4;
                            I7 = I6 + N4;
                            I8 = I7 + N4;
                            T1 = X[I3] * CC1 + X[I7] * SS1;
                            T2 = X[I7] * CC1 - X[I3] * SS1;
                            T3 = X[I4] * CC3 + X[I8] * SS3;
                            T4 = X[I8] * CC3 - X[I4] * SS3;
                            T5 = T1 + T3;
                            T6 = T2 + T4;
                            T3 = T1 - T3;
                            T4 = T2 - T4;
                            T2 = X[I6] + T6;
                            X[I3] = T6 - X[I6];
                            X[I8] = T2;
                            T2 = X[I2] - T3;
                            X[I7] = -X[I2] - T3;
                            X[I4] = T2;
                            T1 = X[I1] + T5;
                            X[I6] = X[I1] - T5;
                            X[I1] = T1;
                            T1 = X[I5] + T4;
                            X[I5] = X[I5] - T4;
                            X[I2] = T1;
                        }
                        IS = 2 * ID - N2;
                        ID = 4 * ID;
                    } while (IS < N);
                }
            }

            // calculate spectrogram
            double[] spectrogram = new double[length/2+1];
            int index = length / 2;
            for (int i = 1; i < spectrogram.Length; i++)
            {
                spectrogram[i] = (X[i] * X[i]) + (X[index + i] * X[index + i]);
            }
            return spectrogram;
        }
        #endregion

    }
}
